/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-18 19:52:44
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-20 09:42:03
 * @FilePath: /esp32_motor/motor/inc/motor_priv.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "motor.h"
#include "pwm.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "AS5600.h"
#include "math_common.h"
#include "esp_timer.h"
#include "driver/gptimer.h"
#include "currentSensing.h"
#include "dataSensing.h"
#include "pid.h"

#define MOTOR_TIMER
#define MOTOR_OBSERVER

#define MOTOR_FLUX 0.00056F
#define MOTOR_RS   0.09F
#define MOTOR_LS   0.00001325F

typedef struct
{
    int      nLaps;
    _iq24    angle;
    uint64_t timeStamp;
    SinCos   valueSinCos;
} MotorAngle;

typedef struct
{
    float setSpeed; /*RW--reg offset 0*/
    float setAngle; /*RW--reg offset 2*/
    float focFreq;  /*R --reg offset 4*/
    float uq;       /*RW--reg offset 6*/
    float speedP;   /*RW--reg offset 8*/
    float speedI;   /*RW--reg offset 10*/
    float speedD;   /*RW--reg offset 12*/
    float iqP;      /*RW--reg offset 14*/
    float iqI;      /*RW--reg offset 16*/
    float iqD;      /*RW--reg offset 18*/
    float idP;      /*RW--reg offset 20*/
    float idI;      /*RW--reg offset 22*/
    float idD;      /*RW--reg offset 24*/
    float ud;       /*RW--reg offset 26*/
    float time;     /*RW--reg offset 26*/
} MotorReg;
typedef struct
{
    MotorReg reg;
    int      startAddr;
} MotorRegArea;

typedef struct
{
    PwmDev          *phase[MOTOR_PHASE_NUM];
    _iq24            zeroElecAngle;
    PhasePara        voltage;
    PhasePara        ampare;
    _iq24            vMax;
    _iq24            _1_vMax;
    _iq21            setSpeed;
    _iq21            realSpeed;
    _iq24            setAngle;
    int              dutyCount; /*占空比最大值*/
    int              pp;
    int              dir;
    AS5600Dev       *sensor;
    MotorAngle       curAngle;
    TaskHandle_t     task;
    QueueHandle_t    modbusQueue;
    MotorRegArea     modbusReg;
    DataSendFrame    dataFrame;
    FilterAlgorithm *speedFilter;
    FilterAlgorithm *iqFilter;
    FilterAlgorithm *idFilter;
    CurrentSensing  *ampereSensing;
    PidControl      *speedPID;
    PidControl      *iqPID;
    PidControl      *idPID;
#ifdef MOTOR_TIMER
    esp_timer_handle_t motorTimer;
#endif
    void (*setPhaseVoltage)(MotorDev *dev);
} MotorObj;